#ifndef BASE_KALMANFILTER_H
#define BASE_KALMANFILTER_H

#include <Eigen/Dense>

#include "postprocess/common/common_func.h"

// 这是个纯虚类，用于kalman滤波的基类
class BaseKalmanFilter {
 public:
  BaseKalmanFilter();
  BaseKalmanFilter(BaseKalmanFilter const&) = delete;
  BaseKalmanFilter& operator=(BaseKalmanFilter const&) = delete;
  BaseKalmanFilter(BaseKalmanFilter&&) = delete;
  BaseKalmanFilter& operator=(BaseKalmanFilter&&) = delete;
  virtual ~BaseKalmanFilter();

  /// @brief Init parameters
  /// @param
  /// @return
  virtual void Init(int dynam_param_num, int measure_param_num,
                    int control_param_num = 0);

  /// @brief kalman predict
  /// @param
  /// @return
  //  virtual Eigen::VectorXf Predict(double time_diff) = 0;

  // @brief update filter with object
  /// @param new_object
  /// @param old_object
  /// @param time_diff
  /// @param is_utm_flag
  virtual void UpdateWithObject(TrackedObjectPtr& new_object,
                                TrackedObjectPtr const& old_object,
                                double time_diff) = 0;

  /// @brief update filter without object
  /// @param time_diff
  virtual void UpdateWithoutObject(double time_diff) = 0;

  /// @brief set Q
  /// @param Matrix Q
  virtual void SetQ(Eigen::MatrixXf& Q) = 0;

  /// @brief set R
  /// @param Matrix R
  virtual void SetR(Eigen::MatrixXf& R) = 0;

 protected:
  /// @brief kalman filter update state with  observation
  /// @param
  virtual void update_state(TrackedObjectPtr const& new_object,
                            TrackedObjectPtr const& old_object,
                            double time_diff) = 0;

  /// @brief adjust kalman gain
  /// @param
  virtual void state_gain_adjustment(Eigen::VectorXf& gain,
                                     double time_diff = 0.0) = 0;

  Eigen::MatrixXf A_;
  //   Eigen::MatrixXf B_;//我们的模型中没有输入
  Eigen::MatrixXf H_;
  Eigen::MatrixXf R_;
  Eigen::MatrixXf Q_;
  Eigen::MatrixXf P_;
  Eigen::VectorXf State_;

  int dynam_param_num_;
  int measure_param_num_;
  int control_param_num_;

  /// @brief filter history
  std::int32_t age_;
};

#endif
